#include "fdc.h"
#include "x86.h"
#include <cstdint.h>
#include "kmm.h"

FloppyMSRT fdc_read_msr(){
	return (FloppyMSRT) inb(FloppyRegisters::MAIN_STATUS_REGISTER);
}

void fdc_write_ccr(uint8_t ccr){
	outb(FloppyRegisters::CONFIGURATION_CONTROL_REGISTER,ccr);
}

void fdc_write_dor(FloppyDORT dor){
	outb(FloppyRegisters::DIGITAL_OUTPUT_REGISTER,(uint8_t) dor);
}

void fdc_send_command(uint8_t command){
	for (int tries=0;tries < FDC_MAX_IO_TRIES;tries++)
		if (fdc_read_msr().mrq)
			return outb(FloppyRegisters::DATA_FIFO,command);
}

uint8_t fdc_read_data(){
	for (int tries=0;tries < FDC_MAX_IO_TRIES;tries++)
		if (fdc_read_msr().mrq)
			return inb(FloppyRegisters::DATA_FIFO);
}

void fdc_interrupt_handler(registers_t regs){
	int_fired = 1;
}

void fdc_await_int(){//TODO:Sleep here
	while(!int_fired);
	int_fired = 0;
}

void fdc_check_int (uint8_t* st0, uint8_t* cyl) { 
	fdc_send_command (FloppyCommands::SENSE_INTERRUPT); 
	*st0 = fdc_read_data ();
	*cyl = fdc_read_data ();
}
int fdc_read_sector(uint8_t drive,uint_t track,uint8_t head,uint8_t sector){
	//TODO: DMA READ
	fdc_send_command(FloppyCommands::READ_DATA | FDC_CMD_CFG_DENS | FDC_CMD_CFG_MTRK | FDC_CMD_CFG_SKIP);
	fdc_send_command(drive | (head << 2));
	fdc_send_command(track);
	fdc_send_command(head);
	fdc_send_command(sector);
	fdc_send_command(FloppySectorLength::F_512);
	fdc_send_command((sector + 1) >= FDD_SECTORS_PER_TRACK) ? (FDD_SECTORS_PER_TRACK : (sector+1))
	fdc_send_command(FloppyGAP3Length::F_3_5);
	fdc_send_command(0xFF);
	fdc_await_int();
	FloppyST0T st0 = (FloppyST0T) fdc_read_data();
	for (int pos=0;pos < 6;pos++)
		fdc_read_data();
	uint8_t val = 0;
	fdc_check_int(&val,&val);
	if (st0.interrupt_id != 0)
		return -1;
	return 0x1000;
}
